home *** CD-ROM | disk | FTP | other *** search
/ Graphics Plus / Graphics Plus.iso / general / raytrace / rayshade / graphtal.lzh / Graphtal.Amiga / TransMatrix.C < prev    next >
C/C++ Source or Header  |  1992-11-19  |  10KB  |  367 lines
  1. /*
  2.  * TransMatrix.C - methods for general 4x3 transformation matrices.
  3.  *
  4.  * Copyright (C) 1992, Christoph Streit (streit@iam.unibe.ch)
  5.  *                     University of Berne, Switzerland
  6.  * Portions Copyright (C) 1990, Jonathan P. Leech
  7.  * All rights reserved.
  8.  *
  9.  * This software may be freely copied, modified, and redistributed
  10.  * provided that this copyright notice is preserved on all copies.
  11.  *
  12.  * You may not distribute this software, in whole or in part, as part of
  13.  * any commercial product without the express consent of the authors.
  14.  *
  15.  * There is no warranty or other guarantee of fitness of this software
  16.  * for any purpose.  It is provided solely "as is".
  17.  *
  18.  */
  19.  
  20. #include "TransMatrix.h"
  21.  
  22. /*
  23.  * Copied from lsys by Jonathan P. Leech.
  24.  */
  25. void SinCos(real alpha, real& sin_a, real& cos_a)
  26. {
  27.   const real tolerance = 1e-10;
  28.  
  29.   sin_a = sin(alpha);
  30.   cos_a = cos(alpha);
  31.  
  32.   if (cos_a > 1-tolerance) {
  33.     cos_a = 1; sin_a = 0;
  34.   }
  35.   else if (cos_a < -1+tolerance) {
  36.     cos_a = -1; sin_a = 0;
  37.   }
  38.  
  39.   if (sin_a > 1-tolerance) {
  40.     cos_a = 0; sin_a = 1;
  41.   }
  42.   else if (sin_a < -1+tolerance) {
  43.     cos_a = 0; sin_a = -1;
  44.   }
  45. }
  46.  
  47. //___________________________________________________________ TransMatrix
  48.  
  49. TransMatrix::TransMatrix()
  50. {
  51.   for (register int i=0; i<4; i++)
  52.     for (register int j=0; j<3; j++)
  53.       m[i][j] = (i == j);
  54. }
  55.   
  56. TransMatrix::TransMatrix(const Vector& v1, const Vector& v2, 
  57.              const Vector& v3)
  58. {
  59.   m[0][0] = v1[0]; m[0][1] = v1[1]; m[0][2] = v1[2];
  60.   m[1][0] = v2[0]; m[1][1] = v2[1]; m[1][2] = v2[2];  
  61.   m[2][0] = v3[0]; m[2][1] = v3[1]; m[2][2] = v3[2];  
  62.   m[3][0] = m[3][1] = m[3][2] = 0;
  63. }
  64.  
  65. TransMatrix::TransMatrix(const Vector&v1, const Vector& v2, 
  66.              const Vector& v3, const Vector& v4)
  67. {
  68.   m[0][0] = v1[0]; m[0][1] = v1[1]; m[0][2] = v1[2];
  69.   m[1][0] = v2[0]; m[1][1] = v2[1]; m[1][2] = v2[2];  
  70.   m[2][0] = v3[0]; m[2][1] = v3[1]; m[2][2] = v3[2];  
  71.   m[3][0] = v4[0]; m[3][1] = v4[1]; m[3][2] = v4[2];  
  72. }
  73.  
  74. TransMatrix::TransMatrix(const TransMatrix& t)
  75. {
  76.   for (register int i=0; i<4; i++)
  77.     for (register int j=0; j<3; j++)
  78.       m[i][j] = t.m[i][j];
  79. }
  80.  
  81. const TransMatrix& TransMatrix::operator=(const TransMatrix& t)
  82. {
  83.   for (register int i=0; i<4; i++)
  84.     for (register int j=0; j<3; j++)
  85.       m[i][j] = t.m[i][j];
  86.   return *this;
  87. }
  88.   
  89. TransMatrix& TransMatrix::operator+=(const TransMatrix& t)
  90. {
  91.   for (register int i=0; i<4; i++)
  92.     for (register int j=0; j<3; j++)
  93.       m[i][j] += t.m[i][j];
  94.   return *this;
  95. }
  96.  
  97. TransMatrix& TransMatrix::operator-=(const TransMatrix& t)
  98. {
  99.   for (register int i=0; i<4; i++)
  100.     for (register int j=0; j<3; j++)
  101.       m[i][j] -= t.m[i][j];
  102.   return *this;
  103. }
  104.  
  105. TransMatrix& TransMatrix::operator*=(const TransMatrix& t)
  106. {
  107.   TransMatrix tmp(*this);
  108.  
  109.   for (register int i=0; i<3; i++) {
  110.     m[0][i] = tmp.m[0][0]*t.m[0][i] + tmp.m[0][1]*t.m[1][i] + tmp.m[0][2]*t.m[2][i];
  111.     m[1][i] = tmp.m[1][0]*t.m[0][i] + tmp.m[1][1]*t.m[1][i] + tmp.m[1][2]*t.m[2][i];
  112.     m[2][i] = tmp.m[2][0]*t.m[0][i] + tmp.m[2][1]*t.m[1][i] + tmp.m[2][2]*t.m[2][i];
  113.     m[3][i] = tmp.m[3][0]*t.m[0][i] + tmp.m[3][1]*t.m[1][i] + tmp.m[3][2]*t.m[2][i] +
  114.               t.m[3][i];
  115.   }
  116.  
  117.   return *this;
  118. }
  119.  
  120. TransMatrix TransMatrix::operator-()
  121. {
  122.   TransMatrix result;
  123.  
  124.   for (register int i=0; i<4; i++)
  125.     for (register int j=0; j<3; j++)
  126.       result.m[i][j] = m[i][j];
  127.   return result;
  128. }
  129.  
  130. TransMatrix TransMatrix::operator+(const TransMatrix& t)
  131. {
  132.   TransMatrix result;
  133.  
  134.   for (register int i=0; i<4; i++)
  135.     for (register int j=0; j<3; j++)
  136.       result.m[i][j] = m[i][j] + t.m[i][j];
  137.   return result;
  138. }
  139.  
  140. TransMatrix TransMatrix::operator-(const TransMatrix& t)
  141. {
  142.   TransMatrix result;
  143.  
  144.   for (register int i=0; i<4; i++)
  145.     for (register int j=0; j<3; j++)
  146.       result.m[i][j] = m[i][j] - t.m[i][j];
  147.   return result;
  148.  
  149. }
  150.  
  151. TransMatrix TransMatrix::operator*(const TransMatrix& t)
  152. {
  153.   TransMatrix result;
  154.  
  155.   for (register int i=0; i<3; i++) {
  156.     result.m[0][i] = m[0][0]*t.m[0][i] + m[0][1]*t.m[1][i] + m[0][2]*t.m[2][i];
  157.     result.m[1][i] = m[1][0]*t.m[0][i] + m[1][1]*t.m[1][i] + m[1][2]*t.m[2][i];
  158.     result.m[2][i] = m[2][0]*t.m[0][i] + m[2][1]*t.m[1][i] + m[2][2]*t.m[2][i];
  159.     result.m[3][i] = m[3][0]*t.m[0][i] + m[3][1]*t.m[1][i] + m[3][2]*t.m[2][i] 
  160.                      + t.m[3][i];
  161.   }
  162.  
  163.   return result;
  164. }
  165.  
  166. /*
  167.  * Compute the inverse of a 3D affine matrix; i.e. a matrix with a dimen-
  168.  * sionality of 4 where the right column has entries (0, 0, 0, 1).
  169.  *
  170.  * This procedures treats the 4 by 4 matrix as ablock matrix and calculates
  171.  * the inverse of one submatrix for a significant performance improvement
  172.  * over a general procedure that can invert any nonsingular matrix:
  173.  *        --      --       --         --
  174.  *        |        | -1    |   -1      |
  175.  *        | A    0 |       |  A      0 |
  176.  *   -1   |        |       |           |
  177.  *  M   = |        |    =  |     -1    |
  178.  *        | C    1 |       | -C A    1 |
  179.  *        |        |       |           |
  180.  *        --      --       --         --
  181.  * 
  182.  * where  M is a 4 by 4 matrix,
  183.  *        A is the 3 by 3 upper left submatrix of M,
  184.  *        C is the 1 by 3 lower left subnatrix of M.
  185.  *
  186.  * Returned value:
  187.  *   1   matrix is nonsingular
  188.  *   0   otherwise
  189.  *
  190.  * Reference GraphicsGems I and Craig Kolbs rayshade.
  191.  */
  192.  
  193. int TransMatrix::invert()
  194. {
  195.   TransMatrix ttmp;
  196.   real det;
  197.  
  198.   /*
  199.    * Calculate the determinant of submatrix A (optimized version:
  200.    * don,t just compute the determinant of A)
  201.    */
  202.   ttmp.m[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
  203.   ttmp.m[1][0] = m[1][0]*m[2][2] - m[1][2]*m[2][0];
  204.   ttmp.m[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
  205.  
  206.   ttmp.m[0][1] = m[0][1]*m[2][2] - m[0][2]*m[2][1];
  207.   ttmp.m[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
  208.   ttmp.m[2][1] = m[0][0]*m[2][1] - m[0][1]*m[2][0];
  209.  
  210.   ttmp.m[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
  211.   ttmp.m[1][2] = m[0][0]*m[1][2] - m[0][2]*m[1][0];
  212.   ttmp.m[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
  213.  
  214.   det = m[0][0]*ttmp.m[0][0] - m[0][1]*ttmp.m[1][0] + m[0][2]*ttmp.m[2][0];
  215.  
  216.   /*
  217.    * singular matrix ?
  218.    */
  219.   if (fabs(det) < EPSILON*EPSILON)
  220.     return 0;
  221.  
  222.   det = 1/det;
  223.  
  224.   /*
  225.    * inverse(A) = adj(A)/det(A)
  226.    */
  227.   ttmp.m[0][0] *= det;
  228.   ttmp.m[0][2] *= det;
  229.   ttmp.m[1][1] *= det;
  230.   ttmp.m[2][0] *= det;
  231.   ttmp.m[2][2] *= det;
  232.  
  233.   det = -det;
  234.  
  235.   ttmp.m[0][1] *= det;
  236.   ttmp.m[1][0] *= det;
  237.   ttmp.m[1][2] *= det;
  238.   ttmp.m[2][1] *= det;
  239.  
  240.   ttmp.m[3][0] = -(ttmp.m[0][0]*m[3][0] + ttmp.m[1][0]*m[3][1] + ttmp.m[2][0]*m[3][2]);
  241.   ttmp.m[3][1] = -(ttmp.m[0][1]*m[3][0] + ttmp.m[1][1]*m[3][1] + ttmp.m[2][1]*m[3][2]);
  242.   ttmp.m[3][2] = -(ttmp.m[0][2]*m[3][0] + ttmp.m[1][2]*m[3][1] + ttmp.m[2][2]*m[3][2]);
  243.   
  244.   *this = ttmp;
  245.  
  246.   return 1;
  247. }
  248.  
  249. /*
  250.  * Compute general rotation matrix. 
  251.  * Adapted from Craig Kolbs rayshade.
  252.  */
  253. void TransMatrix::setRotate(const Vector& v, real alpha)
  254. {
  255.   real sin_a, cos_a, n1, n2, n3;
  256.   Vector n(v.normalized());
  257.  
  258.   SinCos(alpha, sin_a, cos_a);
  259.  
  260.   n1 = n[0]; n2 = n[1]; n3 = n[2];
  261.  
  262.   m[0][0] = (n1*n1 + (1. - n1*n1)*cos_a);
  263.   m[0][1] = (n1*n2*(1. - cos_a) + n3*sin_a);
  264.   m[0][2] = (n1*n3*(1. - cos_a) - n2*sin_a);
  265.   m[1][0] = (n1*n2*(1. - cos_a) - n3*sin_a);
  266.   m[1][1] = (n2*n2 + (1. - n2*n2)*cos_a);
  267.   m[1][2] = (n2*n3*(1. - cos_a) + n1*sin_a);
  268.   m[2][0] = (n1*n3*(1. - cos_a) + n2*sin_a);
  269.   m[2][1] = (n2*n3*(1. - cos_a) - n1*sin_a);
  270.   m[2][2] = (n3*n3 + (1. - n3*n3)*cos_a);
  271.   m[3][0] = m[3][1] = m[3][2] = 0;
  272. }
  273.  
  274. TransMatrix& TransMatrix::rotate(const Vector& v, real alpha)
  275. {
  276.   TransMatrix rot;
  277.   rot.setRotate(v, alpha);
  278.   return this->operator*=(rot);
  279. }
  280.  
  281. TransMatrix& TransMatrix::rotate(Axis a, const real sin_a, const real cos_a)
  282. {
  283.   static real m00, m01, m10, m11, m20, m21, m30, m31;
  284.  
  285.   switch(a) {
  286.   case X:
  287.     m01 = m[0][1]; m11 = m[1][1]; m21 = m[2][1]; m31 = m[3][1]; 
  288.  
  289.     m[0][1] = m01*cos_a-m[0][2]*sin_a; m[0][2] = m01*sin_a+m[0][2]*cos_a;
  290.     m[1][1] = m11*cos_a-m[1][2]*sin_a; m[1][2] = m11*sin_a+m[1][2]*cos_a;
  291.     m[2][1] = m21*cos_a-m[2][2]*sin_a; m[2][2] = m21*sin_a+m[2][2]*cos_a;
  292.     m[3][1] = m31*cos_a-m[3][2]*sin_a; m[3][2] = m31*sin_a+m[3][2]*cos_a;
  293.     break;
  294.  
  295.   case Y:
  296.     m00 = m[0][0]; m10 = m[1][0]; m20 = m[2][0]; m30 = m[3][0]; 
  297.  
  298.     m[0][0] = m00*cos_a-m[0][2]*sin_a; m[0][2] = m00*sin_a+m[0][2]*cos_a;
  299.     m[1][0] = m10*cos_a-m[1][2]*sin_a; m[1][2] = m10*sin_a+m[1][2]*cos_a;
  300.     m[2][0] = m20*cos_a-m[2][2]*sin_a; m[2][2] = m20*sin_a+m[2][2]*cos_a;
  301.     m[3][0] = m30*cos_a-m[3][2]*sin_a; m[3][2] = m30*sin_a+m[3][2]*cos_a;
  302.     break;
  303.  
  304.   case Z:
  305.     m00 = m[0][0]; m10 = m[1][0]; m20 = m[2][0]; m30 = m[3][0]; 
  306.  
  307.     m[0][0] = m00*cos_a-m[0][1]*sin_a; m[0][1] = m00*sin_a+m[0][1]*cos_a;
  308.     m[1][0] = m10*cos_a-m[1][1]*sin_a; m[1][1] = m10*sin_a+m[1][1]*cos_a;
  309.     m[2][0] = m20*cos_a-m[2][1]*sin_a; m[2][1] = m20*sin_a+m[2][1]*cos_a;
  310.     m[3][0] = m30*cos_a-m[3][1]*sin_a; m[3][1] = m30*sin_a+m[3][1]*cos_a;
  311.     break;
  312.  
  313.   default:
  314.     Error(ERR_PANIC, "TransMatrix::rotate illegal value for axis");
  315.   }
  316.   
  317.   return *this;
  318. }
  319.  
  320. TransMatrix& TransMatrix::rotate(Axis a, const real alpha)
  321. {
  322.   real sin_a, cos_a;
  323.  
  324.   SinCos(alpha, sin_a, cos_a);
  325.  
  326.   return this->rotate(a, sin_a, cos_a);
  327. }  
  328.  
  329. TransMatrix& TransMatrix::scale(const real Sx, const real Sy, const real Sz)
  330. {
  331.   for (register int i=0; i<4; i++) {
  332.     m[i][0] *= Sx;
  333.     m[i][1] *= Sy;
  334.     m[i][2] *= Sz;
  335.   }
  336.  
  337.   return *this;
  338. }  
  339.  
  340. TransMatrix& TransMatrix::translate(const Vector& T)
  341. {
  342.   m[3][0] += T[0];     
  343.   m[3][1] += T[1];     
  344.   m[3][2] += T[2];     
  345.  
  346.   return *this;
  347. }  
  348.  
  349. Vector operator*(const Vector &v, const TransMatrix& t)
  350. {
  351.   return Vector(t.m[0][0]*v[0] + t.m[1][0]*v[1] + t.m[2][0]*v[2] + t.m[3][0],
  352.         t.m[0][1]*v[0] + t.m[1][1]*v[1] + t.m[2][1]*v[2] + t.m[3][1],
  353.         t.m[0][2]*v[0] + t.m[1][2]*v[1] + t.m[2][2]*v[2] + t.m[3][2]);
  354. }
  355.  
  356. ostream& operator<<(ostream &os, const TransMatrix& t)
  357. {
  358.   for (register int i=0; i<4; i++) {
  359.     os << "\t( " << t.m[i][0] << ' ' << t.m[i][1] << ' ' <<t.m[i][2] << " )\n";
  360.   }
  361.  
  362.   return os;
  363. }
  364.  
  365.  
  366.  
  367.